%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%本实验中先反变换，再正变换，对比变换前后的Vd，Vq,theta的值是不是对得上，一致表示能成功变换
%反pack变换
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ts = 0.001;                         % 总作用时长 (秒)
T_arr = 2000;                       % 设定时器计数2000个数的时间是0.001秒
Udc = 24;                           % 直流母线电压 24V


% 时间向量
num_periods = 2;                    % 设置要运行的周期数
theta_range = 0 : 0.01 : 2*pi*num_periods; % 从 0 到 2*pi*num_periods，以 0.1 的间隔变化，
                                      % ':' 表示创建等差数列，语法为 start:step:end。
                                      % start 表示数列的起始值；
                                      % step 表示数列的步长；
                                      % end 表示数列的结束值。

num_points = numel(theta_range); % 确定时间点数量，numel 用于获取数组中元素的个数

% 输入参数
% 这里不需要使用循环，因为Vd和Vq的值在整个时间范围内都是常量
Vd = zeros(1, num_points);  % 初始化长度为num_points的全零数组，用于存储Vd
Vq = Udc * ones(1, num_points);   % 初始化长度为num_points的全一数组，用于存储Vq

% 初始化结果数组
Valpha = zeros(1, num_points); % 创建一个长度为 num_points 的全零数组，用于存储 Valpha
Vbeta = zeros(1, num_points);  % 创建一个长度为 num_points 的全零数组，用于存储 Vbeta

% 计算 Valpha 和 Vbeta 随时间的变化
for i = 1:num_points
    theta = theta_range(i); % 获取当前时间点对应的角度值
    Valpha(i) = Vd(i) * cos(theta) - Vq(i) * sin(theta); % 根据反 Park 变换公式计算 Valpha
    Vbeta(i) = Vd(i) * sin(theta) + Vq(i) * cos(theta);  % 根据反 Park 变换公式计算 Vbeta
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 反clark变换
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Va = zeros(1, num_points); % 创建一个长度为 num_points 的全零数组，用于存储 Va
Vb = zeros(1, num_points); % 创建一个长度为 num_points 的全零数组，用于存储 Vb
Vc = zeros(1, num_points); % 创建一个长度为 num_points 的全零数组，用于存储 Vc

% 计算 Va、Vb 和 Vc 随时间的变化
for i = 1:num_points
    Va(i) = Valpha(i);
    Vb(i) = (-Valpha(i) + Vbeta(i) * sqrt(3))/2;
    Vc(i) = (-Valpha(i) - Vbeta(i) * sqrt(3))/2;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 扇区判断
% 扇区   : 1 2 3 4 5 6
% 判断值 : 3 1 5 4 6 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 扇区判断条件
Sector_A = 0;
Sector_B = 0;
Sector_C = 0;

% 扇区判断中间变量
A = 0;
B = 0;
C = 0;

% 扇区判断结果
N = zeros(1, num_points);

for i = 1 : num_points
    Sector_A = Vbeta(i);
    Sector_B = ( sqrt(3)/2 * Valpha(i) ) - ( 1/2 * Vbeta(i) );
    Sector_C = -( sqrt(3)/2 * Valpha(i) ) - ( 1/2 * Vbeta(i) );

    if ( 0 < Sector_A )
        A = 1;
    else
        A = 0;
    end
    if ( 0 < Sector_B )
        B = 1;
    else
        B = 0;
    end
    if ( 0 < Sector_C )
        C = 1;
    else
        C = 0;
    end
    N(i) = 4 * C + 2 * B + A;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算各扇区矢量作用时长
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Tx = zeros(1, num_points);
Ty = zeros(1, num_points);
Tn = zeros(1, num_points);

Temp = sqrt(3) * Ts / Udc;
for i = 1 : num_points
    switch N(i)
    case 3
        Tx(i) = Temp * Sector_B;
        Ty(i) = Temp * Sector_A;
    case 1
        Tx(i) = Temp * (-Sector_B);
        Ty(i) = Temp * (-Sector_C);
    case 5
        Tx(i) = Temp * Sector_A;
        Ty(i) = Temp * Sector_C;
    case 4
        Tx(i) = Temp * (-Sector_A);
        Ty(i) = Temp * (-Sector_B);
    case 6
        Tx(i) = Temp * -Sector_C;
        Ty(i) = Temp * Sector_B;
    case 2
        Tx(i) = Temp * -Sector_C;
        Ty(i) = Temp * -Sector_A;
    otherwise
        disp('出现错误');
    end
    Tn(i) = Ts - Tx(i) - Ty(i);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 转换成定时器初值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T_arr_a = zeros(1, num_points);
T_arr_b = zeros(1, num_points);
T_arr_c = zeros(1, num_points);

for i = 1 : num_points
    switch N(i)
    case 3
        T_arr_a(i) = round( ( ( Ts - Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_b(i) = round( ( ( Ts + Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_c(i) = round( ( ( Ts + Tx(i) + Ty(i) ) / 4 ) / Ts * T_arr );
    case 1
        T_arr_a(i) = round( ( ( Ts + Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_b(i) = round( ( ( Ts - Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_c(i) = round( ( ( Ts + Tx(i) + Ty(i) ) / 4 ) / Ts * T_arr );
    case 5
        T_arr_a(i) = round( ( ( Ts + Tx(i) + Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_b(i) = round( ( ( Ts - Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_c(i) = round( ( ( Ts + Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
    case 4
        T_arr_a(i) = round( ( ( Ts + Tx(i) + Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_b(i) = round( ( ( Ts + Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_c(i) = round( ( ( Ts - Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
    case 6
        T_arr_a(i) = round( ( ( Ts + Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_b(i) = round( ( ( Ts + Tx(i) + Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_c(i) = round( ( ( Ts - Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
    case 2
        T_arr_a(i) = round( ( ( Ts - Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_b(i) = round( ( ( Ts + Tx(i) + Ty(i) ) / 4 ) / Ts * T_arr );
        T_arr_c(i) = round( ( ( Ts + Tx(i) - Ty(i) ) / 4 ) / Ts * T_arr );
    otherwise
        disp('出现错误');
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 操作MOS管
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
U = zeros( 1, T_arr * num_points );
V = zeros( 1, T_arr * num_points );
W = zeros( 1, T_arr * num_points );
time = zeros( 1, T_arr * num_points );
index = 0;
flag = 1;
crr = 1;
Umax = Udc - 1;
for i = 1 : num_points
    for j = 1 : T_arr
        index = (i - 1) * T_arr + j;
        if( flag == 1 )
            if( crr == T_arr )
                flag = 2;
            end
            if( crr > T_arr_a(i) )
                U( index ) = 1 + Umax;
            end
            if( crr > T_arr_b(i) )
                V( index ) = 1 + Umax;
            end
            if( crr > T_arr_c(i) )
                W( index ) = 1 + Umax;
            end
            crr = crr + 1;
        end
        if( flag == 2 )
            crr = crr - 1;
            if( crr == 1 )
                flag = 1;
            end
            if( crr > T_arr_a(i) )
                U( index ) = 1 + Umax;
            end
            if( crr > T_arr_b(i) )
                V( index ) = 1 + Umax;
            end
            if( crr > T_arr_c(i) )
                W( index ) = 1 + Umax;
            end
        end
        time( index ) = index;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SVPWM 经过低通滤波器后的波形
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fs = 1 / Ts; % 采样频率
cutoff_frequency = 50; % 截止频率，单位Hz
filter_order = 4; % 滤波器阶数
[b, a] = butter(filter_order, cutoff_frequency/(fs/2)); % 设计4阶Butterworth低通滤波器

% 使用之前生成的PWM信号进行滤波
filtered_U = filter(b, a, U);
filtered_V = filter(b, a, V);
filtered_W = filter(b, a, W);

% 放大和偏置处理，得到正弦波形
amplitude = 1.5; % 正弦波振幅
offset = 1.5; % 正弦波偏置
sin_wave_U = amplitude * filtered_U + offset;
sin_wave_V = amplitude * filtered_V + offset;
sin_wave_W = amplitude * filtered_W + offset;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% clark变换
% 这里的电压和电流直接赋值是为了验证算法本身，
% 实际上在FOC中电压是输出给电机线圈的，电流是通过ADC采集回来的。
% 电压是PID根据电流反馈和设定值计算出来的
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ia = Va;
Ib = Vb;
Ic = Vc;

% 初始化结果数组
Ialpha = zeros(1, num_points);
Ibeta = zeros(1, num_points);
for i = 1:num_points
    Ialpha(i) = Ia(i);
    Ibeta(i) = (Ia(i) + 2 * Ib(i))/sqrt(3);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% pack变换
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Id = zeros(1, num_points); % 创建一个长度为 num_points 的全零数组，用于存储 Id
Iq = zeros(1, num_points); % 创建一个长度为 num_points 的全零数组，用于存储 Iq

% 计算 Id 和 Iq 随时间的变化
for i = 1:num_points
    theta = theta_range(i); % 获取当前时间点对应的角度值
    Id(i) = Ialpha(i) * cos(theta) + Ibeta(i) * sin(theta); % 根据 Pack 变换公式计算 Id
    Iq(i) = -Ialpha(i) * sin(theta) + Ibeta(i) * cos(theta); % 根据 Pack 变换公式计算 Iq
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 绘制波形图
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure; % 创建一个新的图形窗口
% 绘制反 Park 变换波形
subplot(3, 3, 1); % 创建第一个子图，3行3列，当前绘制的是第一个子图
plot(theta_range, Valpha, 'b', theta_range, Vbeta, 'r'); % 绘制 Valpha 和 Vbeta 随时间的波形图
xlabel('角度 (rad)'); % 设置 x 轴标签
ylabel('电压 (V)');   % 设置 y 轴标签
title('反 Park 变换后的波形图'); % 设置子图标题
legend('Valpha', 'Vbeta'); % 添加图例，标识每条曲线的含义
grid on; % 打开网格线

% 绘制反 Clark 变换波形
subplot(3, 3, 2); % 创建第二个子图，3行3列，当前绘制的是第二个子图
plot(theta_range, Va, 'r', theta_range, Vb, 'g', theta_range, Vc, 'm'); % 绘制 Va、Vb 和 Vc 随时间的波形图
xlabel('角度 (rad)'); % 设置 x 轴标签
ylabel('电压 (V)');   % 设置 y 轴标签
title('反 Clark 变换后的波形图'); % 设置子图标题
legend('Va', 'Vb', 'Vc'); % 添加图例，标识每条曲线的含义
grid on; % 打开网格线


% 绘制 Clark 变换波形
subplot(3, 3, 3); % 创建第三个子图，3行3列，当前绘制的是第三个子图
plot(theta_range, Ialpha, 'b', theta_range, Ibeta, 'r');
xlabel('角度 (rad)'); % 设置 x 轴标签
ylabel('电流 (A)');   % 设置 y 轴标签
title('Clark 变换后的波形图'); % 设置子图标题
legend('Ialpha', 'Ibeta'); % 添加图例，标识每条曲线的含义
grid on; % 打开网格线

% 绘制 Pack 变换波形
subplot(3, 3, 4); % 创建第四个子图，3行3列，当前绘制的是第四个子图
plot(theta_range, Id, 'b', ...
     theta_range, Iq, 'r', ...
     theta_range, Vd, 'g--', ...
     theta_range, Vq, 'k--');
xlabel('角度 (rad)'); % 设置 x 轴标签
ylabel('电流/电压 (A/V)');   % 设置 y 轴标签
title('Pack 变换后的波形图'); % 设置子图标题
legend('Id', 'Iq', 'Vd', 'Vq'); % 添加图例，标识每条曲线的含义
grid on; % 打开网格线

% 绘制扇区判断的波形
subplot(3, 3, 5); % 创建第四个子图，3行3列
plot(theta_range, N, 'b');
xlabel('角度 (rad)');    % 设置 x 轴标签
ylabel('扇区判断值');     % 设置 y 轴标签
title('扇区判断的波形');  % 设置子图标题
legend('Page'); % 添加图例，标识每条曲线的含义
grid on; % 打开网格线

subplot(3, 3, 6);
plot(theta_range, T_arr_a, 'r', ...
     theta_range, T_arr_b, 'g', ...
     theta_range, T_arr_c, 'b');
xlabel('角度 (rad)');  
ylabel('作用时间');     
title('定时器初值');     
legend('T_arr_a', 'T_arr_b', 'T_arr_c'); 
grid on; % 打开网格线

subplot(3, 3, 7);
plot(time, U, 'r', ...
     time, V, 'g', ...
     time, W, 'b');
% 添加标签和标题
xlabel('Time');
ylabel('作用时间');
title('定时器初值');
% 添加图例
legend('U', 'V', 'W');
% 打开网格线
grid on;

subplot(3, 3, 8);
plot(time, sin_wave_U, 'r', time, sin_wave_V, 'g', time, sin_wave_W, 'b');
xlabel('Time');
ylabel('Voltage');
title('Filtered Sinusoidal Waveforms');
legend('U Phase', 'V Phase', 'W Phase');
grid on;

